;; genelate coordinates in util-cds.l
(defun get-openrave-gcds->eus-endcoords-trans (arm &optional (robot :hrp2jsknt))
  (print robot)
  (case robot
    (:hrp2jsknt
     (if (equal arm :larm)
         (let* ((l-wrist-p->openrave-gcds-trans
                 (make-coords :pos (float-vector 0 -30 -170)
                              :rot (make-array (list 3 3)
                                               :element-type float-vector
                                               :initial-contents
                                               (list
                                                (float-vector 1 0 0)
                                                (float-vector 0 0 -1)
                                                (float-vector 0 1 0)))))
                (l-wrist-p->endcoords-trans
                 (send (send *hrp2-16* :larm :wrist-p :copy-worldcoords)
                       :transformation
                       (send *hrp2-16* :larm :end-coords :copy-worldcoords)))
                (openrave-gcds->end-coords-trans
                 (send (send l-wrist-p->openrave-gcds-trans :inverse-transformation)
                       :transform
                       (send l-wrist-p->endcoords-trans :copy-worldcoords))))
           openrave-gcds->end-coords-trans)
       (let* ((r-wrist-p->openrave-gcds-trans
               (make-coords :pos (float-vector 0 30 -170)
                            :rot (make-array (list 3 3)
                                             :element-type float-vector
                                             :initial-contents
                                             (list
                                              (float-vector 1 0 0)
                                              (float-vector 0 0 1)
                                              (float-vector 0 -1 0)))))
              (r-wrist-p->endcoords-trans
               (send (send *hrp2-16* :rarm :wrist-p :copy-worldcoords)
                     :transformation
                     (send *hrp2-16* :rarm :end-coords :copy-worldcoords)))
              (openrave-gcds->end-coords-trans
               (send (send r-wrist-p->openrave-gcds-trans :inverse-transformation)
                     :transform
                     (send r-wrist-p->endcoords-trans :copy-worldcoords))))
         openrave-gcds->end-coords-trans)))
    (:pr2
     (if (equal arm :larm)
         (let* ((l_gripper_palm_link->openrave-gcds-trans
                 (make-coords :pos (float-vector 180 0 0)
                              :rot (make-array (list 3 3)
                                               :element-type float-vector
                                               :initial-contents
                                               (list
                                                (float-vector 0 0 1)
                                                (float-vector 0 1 0)
                                                (float-vector -1 0 0)))))
                (l_gripper_palm_link->endcoords-trans
                 (send (send *pr2* :l_gripper_palm_link :copy-worldcoords)
                       :transformation
                       (send *pr2* :larm :end-coords :copy-worldcoords)))
                (openrave-gcds->end-coords-trans
                 (send (send l_gripper_palm_link->openrave-gcds-trans :inverse-transformation)
                       :transform
                       (send l_gripper_palm_link->endcoords-trans :copy-worldcoords))))
           openrave-gcds->end-coords-trans)
       (let* ((r_gripper_palm_link->openrave-gcds-trans
               (make-coords :pos (float-vector 180 0 0)
                            :rot (make-array (list 3 3)
                                             :element-type float-vector
                                             :initial-contents
                                             (list
                                              (float-vector 0 0 1)
                                              (float-vector 0 1 0)
                                              (float-vector -1 0 0)))))
              (r_gripper_palm_link->endcoords-trans
               (send (send *pr2* :r_gripper_palm_link :copy-worldcoords)
                     :transformation
                     (send *pr2* :rarm :end-coords :copy-worldcoords)))
              (openrave-gcds->end-coords-trans
               (send (send r_gripper_palm_link->openrave-gcds-trans :inverse-transformation)
                     :transform
                     (send r_gripper_palm_link->endcoords-trans :copy-worldcoords))))
         openrave-gcds->end-coords-trans)))
    (:hironx
     (print "hoge")
     (if (equal arm :larm)
         (let* ((larm-link5->openrave-gcds-trans
                 (make-coords :pos (float-vector 0 0 -50)
                              :rot (make-array (list 3 3)
                                               :element-type float-vector
                                               :initial-contents
                                               (list
                                                (float-vector 0 0 -1)
                                                (float-vector 0 1 0)
                                                (float-vector 1 0 0)))))
                (larm-link5->endcoords-trans
                 (send (send *hironx* :link :larm-link5 :copy-worldcoords)
                       :transformation
                       (send *hironx* :larm :end-coords :copy-worldcoords)))
                (openrave-gcds->end-coords-trans
                 (send (send larm-link5->openrave-gcds-trans :inverse-transformation)
                       :transform
                       (send larm-link5->endcoords-trans :copy-worldcoords))))
           openrave-gcds->end-coords-trans)
       (let* ((rarm-link5->openrave-gcds-trans
               (make-coords :pos (float-vector 0 0 -50)
                            :rot (make-array (list 3 3)
                                             :element-type float-vector
                                             :initial-contents
                                             (list
                                              (float-vector 0 0 -1)
                                              (float-vector 0 1 0)
                                              (float-vector 1 0 0)))))
              (rarm-link5->endcoords-trans
               (send (send *hironx* :link :rarm-link5 :copy-worldcoords)
                     :transformation
                     (send *hironx* :rarm :end-coords :copy-worldcoords)))
              (openrave-gcds->end-coords-trans
               (send (send rarm-link5->openrave-gcds-trans :inverse-transformation)
                     :transform
                     (send rarm-link5->endcoords-trans :copy-worldcoords))))
         openrave-gcds->end-coords-trans))
     )))

;;for transform openrave graspplanning result
(setq *l-openrave-gcds->eus-ecds-lst*
      (list
       (list :hrp2jsknt
             (make-coords :pos (float-vector -4.2 45.5 9.2)
                          :rpy (float-vector -1.571 2.220e-16 -1.571)))
       (list :pr2
             (make-coords :rpy (float-vector 0.245 -1.571 -0.245)))
       (list :hironx
             (make-coords :pos (float-vector 50.0 0.0 100.0)
                          :rpy (float-vector 2.88 5.431e-16 3.142)))))

(setq *r-openrave-gcds->eus-ecds-lst*
      (list
       (list :hrp2jsknt
             (make-coords :pos (float-vector -4.2 -45.5 9.2)
                          :rpy (float-vector 1.571 2.220e-16 1.571)))
       (list :pr2
             (make-coords :rpy (float-vector 0.0 -1.571 0.0)))
       (list :hironx
             (make-coords :pos (float-vector 50.0 0.0 100.0)
                          :rpy (float-vector -2.88 5.431e-16 -3.142)))))

(defun make-hrp2jsknt-ecds->hcds-trans ()
  (setq *l-ecds->hcds*
        (send (send *hrp2-16* :larm :end-coords :copy-worldcoords)
              :transformation
              (send *hrp2-16* :larm :hand :copy-worldcoords)))
  
  (setq *r-ecds->hcds*
        (send (send *hrp2-16* :rarm :end-coords :copy-worldcoords)
              :transformation
              (send *hrp2-16* :rarm :hand :copy-worldcoords))))
